"""
六足机器人动作执行器模块

该模块提供了动作执行的核心功能，包括：
- 动作组管理
- 动作执行控制
- 实时动作执行线程
- 逆运动学计算
"""

import logging
import time
import os
import json
import math
import threading
from typing import List, Optional, Tuple

from robot_core.protocols.serial_protocol import SerialProtocol
from robot_core.protocols.servo_protocol import ServoProtocol
from robot_core.protocols.servo_protocol import ServoCommand
from robot_core.config.servo_config import servo_config


class ActionExecutor:
    """动作执行器类"""

    # 机器人腿部参数（单位：mm）
    shoulder_length = 44.60  # 肩关节长度
    thigh_length = 75.00  # 大腿长度
    calf_length = 126.50  # 小腿长度

    def __init__(self):
        """初始化动作执行器"""
        print("初始化动作执行器...")
        self.serial_protocol = SerialProtocol(log_level=logging.DEBUG)
        self.servo_protocol = ServoProtocol(
            self.serial_protocol, log_level=logging.DEBUG
        )
        self.running_action = False
        self.stop_running = True
        self.online_action_num = None
        self.online_action_times = -1
        self.update_ok = False
        self.action_group_finish = True
        self._action_thread = None
        # 获取动作文件所在目录
        self.action_dir = os.path.join(
            os.path.dirname(os.path.abspath(__file__)), "actions"
        )
        print(f"动作文件目录: {self.action_dir}")

        self.servo_protocol.connect()

        # 初始化舵机参数
        self._init_servos()

    def _init_servos(self):
        """初始化所有舵机参数"""
        print("初始化舵机参数...")
        for servo_id in range(1, 19):
            try:
                # 读取舵机当前位置
                pos = self.servo_protocol.get_response(ServoCommand.POS_READ)
                if pos is not None:
                    print(f"舵机 {servo_id} 当前位置: {pos}")

                # 读取舵机温度
                temp = self.servo_protocol.get_response(ServoCommand.TEMP_READ)
                if temp is not None:
                    print(f"舵机 {servo_id} 当前温度: {temp}°C")

                # 读取舵机电压
                vin = self.servo_protocol.get_response(ServoCommand.VIN_READ)
                if vin is not None:
                    print(f"舵机 {servo_id} 当前电压: {vin}mV")

                # 设置舵机角度限制
                self.servo_protocol.write_command(
                    servo_id, ServoCommand.ANGLE_LIMIT_WRITE, [0, 1000]
                )
                time.sleep(0.1)

                # 设置舵机电压限制
                self.servo_protocol.write_command(
                    servo_id, ServoCommand.VIN_LIMIT_WRITE, [4500, 12000]
                )
                time.sleep(0.1)

                # 设置舵机温度限制
                self.servo_protocol.write_command(
                    servo_id, ServoCommand.TEMP_MAX_LIMIT_WRITE, [85]
                )
                time.sleep(0.1)

            except Exception as e:
                import traceback

                traceback.print_exc()
                print(f"初始化舵机 {servo_id} 时出错: {e}")

    def set_servo(self, servo_id: int, position: int, duration: int) -> None:
        """
        设置舵机位置

        Args:
            servo_id: 舵机ID
            position: 目标位置(0-1000)
            duration: 运行时间(ms)
        """
        print(f"移动舵机 {servo_id} 到位置 {position}，时间 {duration}ms")
        try:
            # 检查舵机状态
            temp = self.servo_protocol.get_response(ServoCommand.TEMP_READ)
            if temp is not None and temp > 80:
                print(f"警告: 舵机 {servo_id} 温度过高: {temp}°C")

            vin = self.servo_protocol.get_response(ServoCommand.VIN_READ)
            if vin is not None and vin < 4500:
                print(f"警告: 舵机 {servo_id} 电压过低: {vin}mV")

            # 发送移动命令
            self.servo_protocol.write_command(
                servo_id, ServoCommand.MOVE_TIME_WRITE, [position, duration]
            )

            # 等待运动完成
            time.sleep(duration / 1000.0)

            # 验证位置
            current_pos = self.servo_protocol.get_response(ServoCommand.POS_READ)
            if current_pos is not None:
                print(f"舵机 {servo_id} 当前位置: {current_pos}")

        except Exception as e:
            print(f"设置舵机 {servo_id} 时出错: {e}")
            raise

    def stop_servo(self, servo_id: Optional[int] = None) -> None:
        """
        停止舵机运行

        Args:
            servo_id: 舵机ID，如果为None则停止所有舵机
        """
        if servo_id is None:
            for i in range(18):
                servo_config.stop(i + 1)
        else:
            servo_config.stop(servo_id)

    def stop_action_group(self) -> None:
        """停止当前正在执行的动作组"""
        self.update_ok = False
        self.stop_running = True
        self.online_action_num = None
        self.online_action_times = -1
        time.sleep(0.1)

    def is_action_finished(self) -> bool:
        """
        检查动作组是否执行完成

        Returns:
            bool: 动作组执行状态
        """
        return self.action_group_finish

    def _execute_action(self, action_data: dict) -> None:
        """
        执行单个动作

        Args:
            action_data: 动作数据
        """
        print(f"开始执行动作，共 {len(action_data['steps'])} 步")

        # 检查所有舵机状态
        for servo_id in range(1, 19):
            try:
                temp = self.servo_protocol.get_response(ServoCommand.TEMP_READ)
                vin = self.servo_protocol.get_response(ServoCommand.VIN_READ)
                if temp is not None and vin is not None:
                    print(f"舵机 {servo_id} 状态 - 温度: {temp}°C, 电压: {vin}mV")
            except Exception as e:
                print(f"检查舵机 {servo_id} 状态时出错: {e}")

        for i, step in enumerate(action_data["steps"], 1):
            if self.stop_running:
                break

            print(f"\n执行第 {i} 步:")
            if step["type"] == "servo":
                print(
                    f"执行舵机动作: ID={step['servo_id']}, 位置={step['position']}, 速度={step['speed']}"
                )
                try:
                    # 检查目标位置是否在限制范围内
                    if step["position"] < 0 or step["position"] > 1000:
                        print(
                            f"警告: 舵机 {step['servo_id']} 目标位置 {step['position']} 超出范围"
                        )
                        continue

                    # 发送移动命令
                    self.servo_protocol.write_command(
                        step["servo_id"],
                        ServoCommand.MOVE_TIME_WRITE,
                        [step["position"], step["speed"]],
                    )

                    # 等待运动完成
                    time.sleep(step["speed"] / 1000.0)

                    # 验证位置
                    current_pos = self.servo_protocol.get_response(
                        ServoCommand.POS_READ
                    )
                    if current_pos is not None:
                        print(f"舵机 {step['servo_id']} 当前位置: {current_pos}")

                except Exception as e:
                    print(f"舵机 {step['servo_id']} 动作执行失败: {e}")
                    raise

            elif step["type"] == "leg_position":
                # 新增：支持通过足端位置控制腿部
                print(
                    f"执行腿部位置控制: 腿={step['leg']}, 位置={step['position']}, 速度={step['speed']}"
                )
                try:
                    self.get_angle(step["leg"], step["position"], step["speed"])
                except Exception as e:
                    print(f"腿部 {step['leg']} 动作执行失败: {e}")
                    raise

            elif step["type"] == "delay":
                print(f"延时 {step['duration']}ms")
                time.sleep(step["duration"] / 1000.0)
                print("延时完成")

    def run_action(self, action_name: str) -> None:
        """
        执行动作组

        Args:
            action_name: 动作组名称
        """
        if action_name is None:
            return

        # 构建动作组文件路径
        json_path = os.path.join(self.action_dir, f"{action_name}.json")

        if self.running_action:
            return

        print(f"执行动作: {action_name}")
        print(f"动作文件路径: {json_path}")

        self.running_action = True
        self.stop_running = False  # 重置停止标志
        try:
            if os.path.exists(json_path):
                with open(json_path, "r") as f:
                    action_data = json.load(f)
                print(f"加载的动作数据: {action_data}")
                self._execute_action(action_data)
            else:
                print(f"未能找到动作组文件: {action_name}")
        finally:
            self.running_action = False
            self.stop_running = True  # 恢复停止标志

    def run_action_group(self, action_name: str, times: int = 1) -> None:
        """
        运行动作组，支持停止信号

        Args:
            action_name: 动作组名称
            times: 运行次数，0表示无限循环
        """
        self.stop_running = False
        if not self.action_group_finish:
            return

        if times == 0:
            times = 1
            state = False  # 无限循环模式
        else:
            times = abs(times)
            state = True  # 有限次数模式

        while times:
            if state:
                times -= 1
            if not self.running_action:
                self.run_action(action_name)
            else:
                break

    def _action_thread_func(self) -> None:
        """动作执行线程函数"""
        while True:
            if self.update_ok:
                if self.online_action_times == 0:
                    # 无限次运行
                    self.run_action(self.online_action_num)
                    self.action_group_finish = False
                elif self.online_action_times > 0:
                    # 有次数运行
                    self.run_action(self.online_action_num)
                    self.online_action_times -= 1
                    self.action_group_finish = False
                    if self.online_action_times == 0:
                        self.online_action_times = -1
                else:
                    # 空载
                    self.action_group_finish = True
                    time.sleep(0.01)
            else:
                self.action_group_finish = True
                time.sleep(0.01)

    def start_action_thread(self) -> None:
        """启动动作执行线程"""
        if self._action_thread is None or not self._action_thread.is_alive():
            self._action_thread = threading.Thread(target=self._action_thread_func)
            self._action_thread.daemon = True
            self._action_thread.start()

    def change_action(self, action_name: str, times: int) -> None:
        """
        修改在线执行的动作和次数

        Args:
            action_name: 动作组名称
            times: 执行次数，0表示无限循环
        """
        self.online_action_times = times
        self.online_action_num = action_name
        self.stop_running = False
        self.update_ok = True

    def run_action_sequence(self, action_codes: List[str], times: int = 1) -> None:
        """
        执行动作序列

        Args:
            action_codes: 动作代码列表
            times: 整个序列重复执行的次数，默认为1次
        """
        self.stop_running = False
        if not self.action_group_finish:
            return

        if times == 0:
            times = 1
            state = False
        else:
            times = abs(times)
            state = True

        while times:
            if state:
                times -= 1
            if not self.running_action:
                self.running_action = True
                for action_code in action_codes:
                    if self.stop_running:
                        self.running_action = False
                        break
                    self.run_action(action_code)
                    if self.stop_running:
                        break
            else:
                break

    def close(self) -> None:
        """关闭动作执行器"""
        if self._action_thread and self._action_thread.is_alive():
            self.stop_action_group()
            self._action_thread.join()
        self.servo_protocol.disconnect()

    def set_leg_position(self, leg: int, position: List[float], speed: int) -> None:
        """
        计算并设置单条腿的舵机角度

        Args:
            leg: 腿的编号(0-5)
            position: [x, y, z]坐标，表示足端位置
            speed: 运动速度(ms)
        """
        print(f"计算腿 {leg} 的舵机角度，目标位置: {position}")

        # 角度转换因子
        factor = 180 / math.pi / 0.24

        # 计算第一个关节角度
        angle1 = math.atan(position[1] / position[0])

        # 计算腿长
        leg_length = position[1] / math.sin(angle1)

        # 计算中间变量
        temp = math.pow(position[2], 2) + pow(leg_length - self.shoulder_length, 2)
        ft = math.sqrt(temp)

        # 计算第二个关节角度
        leg_angle1 = math.atan(position[2] / (leg_length - self.shoulder_length))
        leg_angle2 = math.acos(
            (
                math.pow(self.thigh_length, 2)
                + math.pow(ft, 2)
                - math.pow(self.calf_length, 2)
            )
            / (2 * self.thigh_length * ft)
        )
        leg_angle2 = leg_angle1 + leg_angle2

        # 计算第三个关节角度
        leg_angle3 = math.acos(
            (
                math.pow(ft, 2)
                - math.pow(self.thigh_length, 2)
                - math.pow(self.calf_length, 2)
            )
            / (2 * self.thigh_length * self.calf_length)
        )

        # 转换为舵机角度值
        if leg < 3:
            servo1_pos = int(313 + leg_angle1 * factor)
            servo2_pos = int(500 - leg_angle2 * factor)
            servo3_pos = int(687 - leg_angle3 * factor - 5)
        else:
            servo1_pos = int(687 - leg_angle1 * factor)
            servo2_pos = int(500 + leg_angle2 * factor)
            servo3_pos = int(313 + leg_angle3 * factor + 5)

        # 设置舵机位置
        servo_base = leg * 3 + 1
        print(f"设置舵机位置 - ID: {servo_base}, {servo_base+1}, {servo_base+2}")
        print(f"目标位置: {servo1_pos}, {servo2_pos}, {servo3_pos}")

        self.set_servo(servo_base, servo1_pos, speed)
        self.set_servo(servo_base + 1, servo2_pos, speed)
        self.set_servo(servo_base + 2, servo3_pos, speed)

    def stand_up(self, speed: int = 1000) -> None:
        """
        初始化六足机器人站立姿势

        Args:
            speed: 运动速度(ms)，默认1000ms
        """
        print("初始化六足机器人站立姿势...")
        # 初始化六足机器人站立姿势
        for leg in range(6):
            position = [100.0, 100.0, -70.0]
            self.set_leg_position(leg, position, speed)
        time.sleep(speed / 1000.0)
